/*
 * @Author: junhuizhou
 * @Date: 2021-01-09 10:31:25
 * @LastEditor: junhuizhou
 * @LastEditTime: 2021-01-11 17:34:47
 * @Description: header
 * @FilePath: \fzhtq\src\ctllaw.c
 */

#include "../include/ctllaw.h"
#include "../include/constant.h"
#include "../include/calmatrix.h"
#include "../include/caltransmat.h"

void CalTqByQuat(double theta0[3], double omega0[3], double theta1[3], double Tq[3])
{
    /*基于李雅普诺夫稳定性的四元数控制*/
    double q0[4], qc[4], qe[4];
    double matqc[4][4], qcrot[4][4];
    /*初始四元数及目标四元数*/
    Euler2Quat(theta0, q0);
    Euler2Quat(theta1, qc);
    /*姿态四元数*/
    Quat2Qrot(qc, qcrot);
    MatTrans((double*)matqc, (double*)qcrot, 4, 4);
    MatMtp(qe, (double*)matqc, 4, 4, q0, 4, 1);
    double tmp;
    tmp = 1.0/VectorNorm(qe, 4);
    MatMtp(qe, qe, 4, 1, &tmp, 1, 1);
    /*中间变量计算*/
    double JM[3][3] = {{19+20*2.020,0,0}, {0,19+15*2.020,0}, {0,0,19+10*2.020}};
    double JMI[3][3] = {{0.0168,0,0}, {0,0.0203,0}, {0,0,0.0255}};
    double Iw[3], tmpvec[3];
    /*控制率设计*/
    MatMtp(Iw, (double*)JM, 3, 3, omega0, 3, 1);
    VectorCross(tmpvec, omega0, Iw);
    for(int i=0; i<3; i++)
    {
        /* Tq = -K*qe_0*qe_r - D*w + w X Iw -Td */
        tmp = omega0[i] + PID_P_Quat*qe[i+1];
        Tq[i] = -1*PID_P_Quat*qe[0]*qe[i+1] - PID_D_Quat*omega0[i] + tmpvec[i] - Sign(tmp)*Td_Max;
    }
    /*力矩限幅*/
    for(int i=0; i<3; i++)
    {
        if(Tq[i] > MomWheel_Tq_Max) Tq[i] = MomWheel_Tq_Max;
        if(Tq[i] < -MomWheel_Tq_Max) Tq[i] = -MomWheel_Tq_Max;
    }
}

int Sign(double x)
{
    int sign;
    if(x > 0.0)
    {
        sign = 1;
    }
    else if(x < 0.0)
    {
        sign = -1;
    }
    else
    {
        sign = 0;
    }
    return sign;
}
